﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
//using System.Threading.Tasks;
using AMEE.PanTiltControl;
using System.Threading;

namespace AMEE.HalsDemoConsole
{
    class Program
    {
        static void Main(string[] args) {
            Console.WriteLine(getHelp());
            PanTilt controller = new PanTilt();
            //startSequenz(controller);
            
            //waitingSequenz(controller);

            Console.WriteLine("PanTiltControl Demo");
            Console.WriteLine("-------------------");
            Console.WriteLine("Pan  min, neutral, max: {0} / {1} / {2}", PanTilt.PAN_MIN, PanTilt.PAN_NEUTRAL, PanTilt.PAN_MAX);
            Console.WriteLine("Tilt min, neutral, max: {0} / {1} / {2}", PanTilt.TILT_MIN, PanTilt.TILT_NEUTRAL, PanTilt.TILT_MAX);
            string cmdLine = "";
            int num = -1;
            do {
                Console.WriteLine("p[target (uint)], t[target (uint)], s, w, m[pan (rad)];[tilt (rad)] q:");
                cmdLine = Console.ReadLine().ToLower();
                if (cmdLine.StartsWith("m")) {
                    string[] pt = cmdLine.Substring(1, cmdLine.Length - 1).Split(';');
                    if (pt.Length != 2)
                        continue;
                    float p = 0f;
                    float t = 0f;
                    if (Single.TryParse(pt[0], out p)) {
                        if (Single.TryParse(pt[1], out t)) {
                            controller.Move(p, t);
                            controller.Wait(500);
                        }
                    }

                    if (Int32.TryParse(cmdLine.Substring(1, cmdLine.Length - 1), out num)) {
                        controller.TargetTilt = num;
                        Thread.Sleep(500);
                    }
                }
                else if (cmdLine.StartsWith("t")) {
                    if (Int32.TryParse(cmdLine.Substring(1,cmdLine.Length-1), out num)) {
                        controller.TargetTilt = num;
                        Thread.Sleep(500);
                    }
                }
                else if (cmdLine.StartsWith("p")) {
                    if (Int32.TryParse(cmdLine.Substring(1, cmdLine.Length - 1), out num)) {
                        controller.TargetPan = num;
                        Thread.Sleep(500);
                    }
                }
                else if (cmdLine == "s")
                    startSequenz(controller);
                else if (cmdLine == "w")
                    waitingSequenz(controller);
                else
                    Console.WriteLine("unbekannter Befehl");
            } while (cmdLine.ToLower() != "q");

            //Console.WriteLine("Press any key to quit");
            //Console.ReadKey();
        }

        static string getHelp() {
            String msg =
                "Konsolen Demo der Halssteuerung" + Environment.NewLine +
                "Version: " + System.Reflection.Assembly.GetExecutingAssembly().GetName().Version.ToString() + "\n";
            return msg;
        }



        static void startSequenz(PanTilt controller) {
            Console.WriteLine("Startsequenz");
            controller.TargetPan = PanTilt.PAN_MIN;
            controller.TargetTilt = PanTilt.TILT_MIN;
            System.Threading.Thread.Sleep(2000);
            controller.TargetPan = PanTilt.PAN_NEUTRAL;
            controller.TargetTilt = PanTilt.TILT_NEUTRAL;
            System.Threading.Thread.Sleep(2000);
            controller.TargetPan = PanTilt.PAN_MAX;
            controller.TargetTilt = PanTilt.TILT_MAX;
            System.Threading.Thread.Sleep(2000);
            controller.TargetPan = PanTilt.PAN_NEUTRAL;
            controller.TargetTilt = PanTilt.TILT_NEUTRAL;
            System.Threading.Thread.Sleep(3000);
        }


        static void waitingSequenz(PanTilt controller) {
            Console.WriteLine("wartende Sequenz");
            Console.WriteLine("\t go to Min");
            controller.TargetPan = PanTilt.PAN_MIN;
            controller.TargetTilt = PanTilt.TILT_MIN;
            controller.Wait();
            Console.WriteLine("\t go to Max");
            controller.TargetPan = PanTilt.PAN_MAX;
            controller.TargetTilt = PanTilt.TILT_MAX;
            controller.Wait();
            Console.WriteLine("\t go to Neutral");
            controller.TargetPan = PanTilt.PAN_NEUTRAL;
            controller.TargetTilt = PanTilt.TILT_NEUTRAL;
            controller.Wait();
        }
    }
}
